import json
import logging
import copy

from .navpoint import Navpoint
from .fusion import Fusion


class DeeplabplusAPI:

    def __init__(self, cfg_path):
        with open(cfg_path) as file:
            cfg = json.load(file)

        engine_shufix = cfg["engine_pth"].split(".")[-1]
        if engine_shufix in ["pth", "pt"]:
            from deeplabplus.pytorch_infer import PytorchInfer
            self.model = PytorchInfer(cfg["engine_pth"], cfg["infer"])
        else:
            from deeplabplus.tensorrt_infer import TensorRTInfer
            self.model = TensorRTInfer(cfg["engine_pth"], cfg["infer"])

        self.nav = Navpoint(
            min_area=7000,
            min_area_threshold=11000,
            # point_filter_func="gauss"
        )
        self.fusion = Fusion(fov=(-45, 45), pad_val=9)

        self.out_width = 640
        self.out_height = 360

        self.logger = logging.getLogger("Planner")

    def inference(self, frame, lidar, navigation_status, target_point=None):
        TURN_NONE = 0
        TURN_LEFT = 1
        TURN_RIGHT = 2
        # navigation_status = TURN_NONE

        # tensorRT模型初始化
        # self.model.initialize()

        # 图像分割
        seg_result = self.model.infer(frame).cpu()
        seg_result_copy = copy.deepcopy(seg_result)

        # 融合雷达障碍物数据
        fusion_result = self.fusion.mask_obstacle_angles(seg_result, lidar)

        # 对当前模型结果进行重新映射
        remap_fusion_result = self.model.remap_labels(fusion_result)

        # 计算导航点
        point = self.nav.select_target_region(remap_fusion_result, target_point)

        if point:
            # 将导航点坐标做归一化处理，归一化坐标到 [0, 1] 范围
            x, y = point
            float_x = x / remap_fusion_result.shape[1]  # 宽
            float_y = y / remap_fusion_result.shape[0]  # 高
            navigation_status = TURN_NONE
        else:
            # 视觉导航点未找到时，使用雷达障碍物数据控制转向
            float_x = -1
            float_y = -1

            # analyze the obstacle set
            contained_ratio, right_obstacle_angle, left_obstacle_angle = (
                self.analyze_intervals(lidar)
            )

            self.logger.verbose(
                "contained_ratio: %.3f, right_obstacle_angle: %f, left_obstacle_angle: %f",
                contained_ratio,
                right_obstacle_angle,
                left_obstacle_angle,
            )

            if contained_ratio > 0.1:
                if navigation_status == TURN_NONE:
                    # 右边障碍物多，就左转；左边障碍物多，就右转
                    if right_obstacle_angle > left_obstacle_angle:
                        navigation_status = TURN_LEFT
                    else:
                        navigation_status = TURN_RIGHT

                if navigation_status == TURN_LEFT:
                    float_x = 0.25
                    float_y = 1
                elif navigation_status == TURN_RIGHT:
                    float_x = 0.75
                    float_y = 1
        adjust_point = [float_x, float_y]
        return point, adjust_point, fusion_result, seg_result_copy, navigation_status

    def analyze_intervals(self, interval_list):
        """
        分析区间列表，返回三个关键结果：
        1. 是否包含(-45, 45)区间
        2. 小于-45的总度数
        3. 大于45的总度数
        """
        # 目标区间和边界
        target_interval = (-30, 30)
        less_than_neg30 = (-90, -30)
        greater_than_30 = (30, 90)

        # 1. 计算包含在-30到30之间的区间
        total_between_neg30_and_30 = 0
        for interval in interval_list:
            # 计算当前区间与(-30, 30)的重叠部分
            overlap_start = max(interval[0], target_interval[0])
            overlap_end = min(interval[1], target_interval[1])
            if overlap_start < overlap_end:
                total_between_neg30_and_30 += overlap_end - overlap_start

        # 2. 计算小于-30的度数
        total_less_neg30 = 0
        for interval in interval_list:
            # 计算当前区间与(-90, -30)的重叠部分
            overlap_start = max(interval[0], less_than_neg30[0])
            overlap_end = min(interval[1], less_than_neg30[1])
            if overlap_start < overlap_end:
                total_less_neg30 += overlap_end - overlap_start

        # 3. 计算大于30的度数
        total_greater_30 = 0
        for interval in interval_list:
            # 计算当前区间与(30, 90)的重叠部分
            overlap_start = max(interval[0], greater_than_30[0])
            overlap_end = min(interval[1], greater_than_30[1])
            if overlap_start < overlap_end:
                total_greater_30 += overlap_end - overlap_start

        return (
            total_between_neg30_and_30 / (target_interval[1] - target_interval[0]),
            total_less_neg30,
            total_greater_30,
        )
